/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
#include "main.h"
#ifdef FLOATING_POINT
#error floating_point
#endif
#include "acb_code.h"
#include "codeparm.h"
#include "error.h"
#include "scb_code.h"
#include "code_lsf.h"

#define NPARAMS 10+(4*4)+3

//int BitAllocs[NPARAMS] = {
//      3, 4, 4, 4, 4, 3, 3, 3, 3, 3,   /* For LSFs */
//      8, 6, 8, 6,                     /* For ACB Delay */
//      5, 5, 5, 5,                     /* For ACB Gain */
//      9, 9, 9, 9,                     /* For SCB Index */
//      5, 5, 5, 5,                     /* For SCB Gain */
//      1,                              /* For Bishnu Bit */
//      1                               /* For Sync Bit */
//};


/*  FS1016 compatible bit allocations */
int BitAllocs[NPARAMS] = {
        3, 4, 4, 4, 4, 3, 3, 3, 3, 3,   /* For LSFs */
        8, 5, 9, 5,                     /* For first subframe parameters */
        6, 5, 9, 5,                     /* For second subframe parameters */
        8, 5, 9, 5,                     /* For third subframe parameters */
        6, 5, 9, 5,                     /* For fourth subframe parameters */
        1,                              /* For Bishnu Bit */
        4,                              /* For Hamming bits */
        1                               /* For Sync Bit */
};

STATIC void Encode(
TX_PARAM        parameters,
TX_BITS         *bits);

STATIC void Decode(
TX_BITS         bits,
int             Assurance,
int             SmoothFlag,
TX_PARAM        *parameters) ;

STATIC void Pack(
int             NParams,
int             Params[],
int             BitAllocs[],
int             *NumPacked,
int             Channel[]);

STATIC void Unpack(
int             NParams,
int             BitAllocs[],
int             Channel[],
int             *NumUnpacked,
int             Params[]);

STATIC void ACBDelayEncode(fxpt_16 Udelay, int *Qdelay);

STATIC void ACBDelayDecode(int Qdelay, fxpt_16 *Udelay);

STATIC int FS1016Encode(TX_BITS InStruct, int OutArray[]);

STATIC int FS1016Decode(int InArray[], TX_BITS *OutStruct);




/**************************************************************************
*                                                                         *
* ROUTINE
*               EncodeParameters
*
* FUNCTION
*               Encode and Pack CELP parameters
* SYNOPSIS
*               EncodeParameters(parameters, bit_len, error_parms, edac,
*                                       frame_num, channel)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       parameters      TX_PARAM i      Frame of CELP parameters
*       bit_len         int      i      Number of bits to encode
*       error_parms     char     i      Name of error parameter file
*       edac            int      i      EDAC flag
*       frame_num       int      i      Frame number
*       channel         int      o      Frame of bits from analysis
*
**************************************************************************/

#include "bitperm.h"

void EncodeParameters(
TX_PARAM        parameters,
int             edac,
uint8           wire_bits[CELP_WIREREP_BYTES])
{
TX_BITS         bits;
int             BitChannel[CELP_WIREREP_BITS];
int             NumPacked;
int             FSParms[NPARAMS];
int             i;

FXPT_PUSHSTATE("EncodeParameters", -1.0, -1.0);
/*  Convert TX parameters to fixed point values that can be transmitted */
        Encode(parameters, &bits);

/*  Put these parameters into FS 1016 compatible form */
        if (FS1016Encode(bits, FSParms) != NPARAMS)
          CELP_PRINTF(("*** Problem Encoding to FS1016 ***\n"));

/*  Pack these values into a binary stream */
        Pack(NPARAMS, FSParms, BitAllocs, &NumPacked, BitChannel);

/*  Perform error protection coding */
          ErrorEncode(BitChannel, edac);

/*  Permute the stream if no EDAC */
        for (i = 0; i < CELP_BYTES; i++)        {
          wire_bits[i] =
            BitChannel[ BitPermute[i * 8 + 0] - 1 ] << 0 |
            BitChannel[ BitPermute[i * 8 + 1] - 1 ] << 1 |
            BitChannel[ BitPermute[i * 8 + 2] - 1 ] << 2 |
            BitChannel[ BitPermute[i * 8 + 3] - 1 ] << 3 |
            BitChannel[ BitPermute[i * 8 + 4] - 1 ] << 4 |
            BitChannel[ BitPermute[i * 8 + 5] - 1 ] << 5 |
            BitChannel[ BitPermute[i * 8 + 6] - 1 ] << 6 |
            BitChannel[ BitPermute[i * 8 + 7] - 1 ] << 7 ;
        }
FXPT_POPSTATE();
}


/**************************************************************************
*                                                                         *
* ROUTINE
*               DecodeParameters
*
* FUNCTION
*               Decode and Unpack CELP parameters
* SYNOPSIS
*               DecodeParameters(bits, num_bits, error_parms, edac,
                        FrameNum, SmoothFlag, parameters)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       channel         int      i      Frame of bits from channel
*       num_bits        int      i      Number of bits in channel
*       error_parms     char     i      Name of error parameter file
*       edac            int      i      EDAC flag
*       FrameNum        int      i      Frame Number
*       SmoothFlag      int      i      User's smoothing preference
*       parameters      TX_PARAM o      Frame of CELP parameters
*
**************************************************************************/

void DecodeParameters(
const uint8     wire_bits[CELP_WIREREP_BYTES],
int             edac,
int             SmoothFlag,
TX_PARAM        *parameters)
{
TX_BITS         bits;
int             BitChannel[CELP_WIREREP_BITS];
int             NumUnpacked;
int             FSParms[NPARAMS];
int             i;
int             Assurance;

/*  Unpermute the stream  */
        for (i = 0; i < CELP_BYTES; i++) {
          BitChannel[BitPermute[i * 8 + 0]-1] = (wire_bits[i] >> 0) & 0x1;
          BitChannel[BitPermute[i * 8 + 1]-1] = (wire_bits[i] >> 1) & 0x1;
          BitChannel[BitPermute[i * 8 + 2]-1] = (wire_bits[i] >> 2) & 0x1;
          BitChannel[BitPermute[i * 8 + 3]-1] = (wire_bits[i] >> 3) & 0x1;
          BitChannel[BitPermute[i * 8 + 4]-1] = (wire_bits[i] >> 4) & 0x1;
          BitChannel[BitPermute[i * 8 + 5]-1] = (wire_bits[i] >> 5) & 0x1;
          BitChannel[BitPermute[i * 8 + 6]-1] = (wire_bits[i] >> 6) & 0x1;
          BitChannel[BitPermute[i * 8 + 7]-1] = (wire_bits[i] >> 7) & 0x1;
        }

/*  Check for errors and correct */
        ErrorDecode(BitChannel, edac, &Assurance);

/*  Unpack the corrected binary stream */
        Unpack(NPARAMS, BitAllocs, BitChannel, &NumUnpacked, FSParms);

/*  Translate from FS1016 compatible mode */
        if (FS1016Decode(FSParms, &bits) != NPARAMS)
          CELP_PRINTF(("*** Problem Decoding from FS1016 ***\n"));

/*  Decode RX bits into parameters to be synthesized */
        Decode(bits, Assurance, SmoothFlag, parameters);

}

/**************************************************************************
*                                                                         *
* ROUTINE
*               Encode
*
* FUNCTION
*               Encode CELP parameters
* SYNOPSIS
*               Encode(parameters, bits)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       parameters      TX_PARAM i      Frame of CELP parameters
*       bits            TX_BITS  o      Frame of bits from analysis
*
**************************************************************************/

void Encode(
TX_PARAM        parameters,
TX_BITS         *bits)
{
int             sf;
static int      sync=0;
fxpt_16         qlsf[ORDER];

FXPT_PUSHSTATE("Encode", -1.0, -1.0);
/*  Convert LSFs to int representation */
        QuanLSF(parameters.lsf, qlsf, bits->lsf);

        for(sf=0; sf < NUM_SF; sf++)    {
/*  Convert ACB gain to int representation */
          ACBGainEncode(parameters.AdaptiveGain[sf], &bits->AdaptiveGain[sf]);

/*  Convert ACB delay to int representation */
          ACBDelayEncode(parameters.AdaptiveDelay[sf],
                &bits->AdaptiveDelay[sf]);

/*  Convert SCB gain to int representation */
          SCBGainEncode(parameters.StochasticGain[sf], &bits->StochasticGain[sf]);

/*  Convert SCB index to int representation */
          bits->StochasticIndex[sf] = parameters.StochasticIndex[sf]-1;
        }

/*  Encode Bishnu bit */
        bits->Bishnu = 0;

/*  Encode sync bit */
        bits->Sync = sync;

/*  Update sync bit */
        sync = (sync == 0) ? 1 : 0;

FXPT_POPSTATE();
}


/**************************************************************************
*                                                                         *
* ROUTINE
*               Decode
*
* FUNCTION
*               Decode CELP parameters
* SYNOPSIS
*               Decode(bits, Assurance, FrameNum, SmoothFlag, parameters)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       bits            TX_BITS  i      Frame of bits from analysis
*       Assurance       int      i      Assurance value from EDAC
*       FrameNum        int      i      Frame number
*       SmoothFlag      int      i      User's smoothing preference
*       parameters      TX_PARAM o      Frame of CELP parameters
*
**************************************************************************/

void Decode(
TX_BITS         bits,
int             Assurance,
int             SmoothFlag,
TX_PARAM        *parameters)
{
int     sf;

/*  Convert LSFs from int representation */
        UnquanLSF(bits.lsf, ORDER, parameters->lsf);

        for(sf=0; sf<NUM_SF; sf++)      {
/*  Convert ACB gain from int representation */
          ACBGainDecode(bits.AdaptiveGain[sf], &parameters->AdaptiveGain[sf]);

/*  Convert ACB delay from int representation */
          ACBDelayDecode(bits.AdaptiveDelay[sf], &parameters->AdaptiveDelay[sf]);

/*  Convert SCB gain from int representation */
          SCBGainDecode(bits.StochasticGain[sf], &parameters->StochasticGain[sf]);

/*  Convert SCB index from int representation */
          parameters->StochasticIndex[sf] = bits.StochasticIndex[sf]+1;

#ifdef FLOATING_POINT
/*  Smooth if necessary and user reqests it */
          if (SmoothFlag)       {
            Smooth(Assurance, FrameNum, sf, parameters);
          }
#endif


        }


}

/**************************************************************************
*                                                                         *
* ROUTINE
*               Pack
*
* FUNCTION
*               Pack CELP parameters
* SYNOPSIS
*               Pack(NParams, Params, BitAllocs, NumPacked, Channel)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       NParams         int      i      Number of parameters to pack
*       Params          int      i      Frame of encoded analysis parameters
*       BitAllocs       int      i      Bit allocations for each parameter
*       NumPacked       int      o      Number of bits packed
*       Channel         int      o      Frame of channel bits
*
**************************************************************************/

void Pack(
int             NParams,
int             Params[],
int             BitAllocs[],
int             *NumPacked,
int             Channel[])
{
int             i, j, k=0;

        for(i=0; i<NParams; i++) { /* For each parameter */
          for(j=0;j<BitAllocs[i]; j++) { /* For each bit in each parameter */
            Channel[k++] = (Params[i] & (1<<j)) >> j;
          }
        }

        *NumPacked = k;

}

/**************************************************************************
*                                                                         *
* ROUTINE
*               Unpack
*
* FUNCTION
*               Unpack CELP parameters
* SYNOPSIS
*               Unpack(NParams, BitAllocs, Channel, NumUnpacked, Params)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       NParams         int      i      Number of parameters to unpack
*       BitAllocs       int      i      Bit allocations for each parameter
*       Channel         int      i      Frame of channel bits
*       NumUnpacked     int      o      Number of bits unpacked
*       Params          int      o      Frame of analysis parameters
*
**************************************************************************/

void Unpack(
int             NParams,
int             BitAllocs[],
int             Channel[],
int             *NumUnpacked,
int             Params[])
{
int     i, j, k=0;

        for(i=0; i<NParams; i++) { /* For each parameter */
          Params[i] = 0;
          for(j=0; j<BitAllocs[i]; j++) { /* For each bit in each parameter */
            Params[i] |= Channel[k++] << j;
          }
        }

        *NumUnpacked = k;

}

/**************************************************************************
*                                                                         *
* ROUTINE
*               ADelayEncode
*
* FUNCTION
*               Encode the Adaptive Delay
* SYNOPSIS
*               ADelayEncode(UDelay, QDelay)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       UDelay          fxpt_16  i      Unquantized adaptive delay
*       QDelay          int      o      Quantized adaptive delay
*
**************************************************************************/

#include "adencode.h"
extern fxpt_16 DelayTable[MAX_NUM_ADAPT];

void ACBDelayEncode(fxpt_16 Udelay, int *Qdelay)
{
        int             i, j;
        static int      even=FALSE;
        static int      LastIndex=1;
        int             MinDelayPtr, MaxDelayPtr;

        if(!even) {
                /* On odd frames search thru delay table until match found */
                for(i=0;i<MAX_NUM_ADAPT;i++) {
                        if (Udelay == DelayTable[i]) {
                                LastIndex = i;
                                break;
                        }
                }

                /*  Retrieve encoded delay value from table */
                *Qdelay = adencode[i];
        }
        else {
                /*  On even frames search for delta delay */
                MinDelayPtr = LastIndex - (NUM_DELTA_ADELAYS / 2 - 1);
                MaxDelayPtr = LastIndex + (NUM_DELTA_ADELAYS / 2);
                if (MinDelayPtr < 0) {
                        MinDelayPtr = 0;
                        MaxDelayPtr = NUM_DELTA_ADELAYS - 1;
                }
                if (MaxDelayPtr > NUM_FULL_ADELAYS-1) {
                        MaxDelayPtr = NUM_FULL_ADELAYS - 1;
                        MinDelayPtr = NUM_FULL_ADELAYS - NUM_DELTA_ADELAYS;
                }
                for(i=MinDelayPtr, j=0; i<=MaxDelayPtr; i++, j++) {
                        if (Udelay == DelayTable[i]) {
                                *Qdelay = j;
                                break;
                        }
                }
        }

        /*  Update even flag */
        even = (even) ? FALSE : TRUE;
}

/**************************************************************************
*                                                                         *
* ROUTINE
*               ACBDelayDecode
*
* FUNCTION
*               Decode the Adaptive Delay
* SYNOPSIS
*               ACBDelayDecode(QDelay, UDelay)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       QDelay          int      i      Quantized adaptive delay
*       UDelay          fxpt_16  o      Unquantized adaptive delay
*
**************************************************************************/

#include "addecode.h"

void ACBDelayDecode(int QDelay, fxpt_16 *UDelay)
{
        static int      even = FALSE;
        static int      LastIndex = 0;
        static int      first = TRUE;
        static int      DeltaDecodeTable[NUM_FULL_ADELAYS];
        int             MaxDelayPtr, MinDelayPtr;
        int             i;

        /*  Generate table for delta delay decoding */
        if(first) {
                first = FALSE;
                for (i = 0; i < NUM_FULL_ADELAYS; i++) {
                        DeltaDecodeTable[adencode[i]] = i;
                }
        }

        if (!even) {
                /*  On even subframes decode from table */
                *UDelay = addecode[QDelay];
                LastIndex = DeltaDecodeTable[QDelay];
        }
        else {
                /*  On odd subframes decode relative to previous frame */
                MinDelayPtr = LastIndex - (NUM_DELTA_ADELAYS / 2 - 1);
                MaxDelayPtr = LastIndex + (NUM_DELTA_ADELAYS / 2);
                if (MinDelayPtr < 0) {
                        MinDelayPtr = 0;
                        MaxDelayPtr = NUM_DELTA_ADELAYS - 1;
                }
                if (MaxDelayPtr > NUM_FULL_ADELAYS-1) {
                        MaxDelayPtr = NUM_FULL_ADELAYS - 1;
                        MinDelayPtr = NUM_FULL_ADELAYS - NUM_DELTA_ADELAYS;
                }
                *UDelay = DelayTable[MinDelayPtr + QDelay];
        }

        /*  Update even */
        even = (even) ? FALSE:TRUE;
}

/**************************************************************************
*                                                                         *
* ROUTINE
*               FS1016Encode
*
* FUNCTION
*               Translate from bit structure to FS1016 compatible array
* SYNOPSIS
*               FS1016Encode(InStruct, OutArray)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       InStruct        TX_BITS  i      Structure of quantized parameters
*       OutArray        int      o      Array of FS1016 compatible parameters
*
**************************************************************************/

int FS1016Encode(
TX_BITS InStruct,
int     OutArray[])
{
int     i, j=0;

FXPT_PUSHSTATE("FS1016Encode", -1.0, -1.0);
/*  Put the LSFs into the array */
        for(i=0;i<ORDER;i++)    {
          OutArray[j++] = InStruct.lsf[i];
        }

/*  Put each subframe's parameters into the array */
        for(i=0;i<NUM_SF;i++)   {
          OutArray[j++] = InStruct.AdaptiveDelay[i];
          OutArray[j++] = InStruct.AdaptiveGain[i];
          OutArray[j++] = InStruct.StochasticIndex[i];
          OutArray[j++] = InStruct.StochasticGain[i];
        }

/*  Now put in the Bishnu bit */
        OutArray[j++] = InStruct.Bishnu;

/*  Next the Hamming Code */
        OutArray[j++] = InStruct.Hamming;

/*  And last the sync bit */
        OutArray[j] = InStruct.Sync;

FXPT_POPSTATE();
/*  Let the calling routine know how many parameters were filled */
        return(j+1);

}

/**************************************************************************
*                                                                         *
* ROUTINE
*               FS1016Decode
*
* FUNCTION
*               Translate to bit structure from FS1016 compatible array
* SYNOPSIS
*               FS1016Encode(InArray, OutStruct)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       InArray         int      i      Array of FS1016 compatible parameters
*       OutStruct       TX_BITS  o      Structure of quantized parameters
*
**************************************************************************/

int FS1016Decode(
int     InArray[],
TX_BITS *OutStruct)
{
int     i, j=0;

/*  Get the LSFs from the array */
        for(i=0;i<ORDER;i++)    {
          OutStruct->lsf[i] = InArray[j++];
        }

/*  Put each subframe's parameters into the array */
        for(i=0;i<NUM_SF;i++)   {
          OutStruct->AdaptiveDelay[i] = InArray[j++];
          OutStruct->AdaptiveGain[i] = InArray[j++];
          OutStruct->StochasticIndex[i] = InArray[j++];
          OutStruct->StochasticGain[i] = InArray[j++];
        }

/*  Now put in the Bishnu bit */
        OutStruct->Bishnu = InArray[j++];

/*  Next the Hamming Code */
        OutStruct->Hamming = InArray[j++];

/*  And last the sync bit */
        OutStruct->Sync = InArray[j];


/*  Let the calling routine know how many parameters were filled */
        return(j+1);
}
